#include "drv_motor_GO_M8010_6.h"
#include <math.h>
#include "crc_ccitt.h"
#include "usart.h"
#include "tim.h"
#include "delay.h"

#define REDUCTION_RADIO 6.33333333f

extern DMA_HandleTypeDef hdma_usart2_rx;

#define SATURATE(_IN, _MIN, _MAX) \
    {                             \
        if (_IN < _MIN)           \
            _IN = _MIN;           \
        else if (_IN > _MAX)      \
            _IN = _MAX;           \
    }

void Motor_GO_M8010_6_Init(Motor_COM_t *motor_com, Device_Motor_t *dev_motor_t, UART_HandleTypeDef *motor_huart, uint8_t Motor_ID)
{
    dev_motor_t->motor_state.motor_id = Motor_ID;
    dev_motor_t->motor_cmd.motorType = GO_M8010_6;
    dev_motor_t->motor_cmd.ID = Motor_ID;
    dev_motor_t->motor_cmd.mode = FOC;
    dev_motor_t->motor_cmd.tor_des = 0.0f;
    dev_motor_t->motor_cmd.spd_des = 0.0f;
    dev_motor_t->motor_cmd.pos_des = 0.0f;
    dev_motor_t->motor_cmd.kp_des = 0.1f;
    dev_motor_t->motor_cmd.kd_des = 0.05f;
    motor_com->motor_huart = motor_huart;

    // printf("Motor_%d init successfully!", dev_motor_t->motor_cmd.ID);
}

int packSetVal2Msg(GO_M8010_6_ControlData *cmdMsg, MotorSetValTypeDef *setVal)
{
    cmdMsg->head[0] = 0xfe;
    cmdMsg->head[1] = 0xee;
    cmdMsg->mode.id = setVal->ID;
    cmdMsg->mode.status = setVal->mode;
    cmdMsg->mode.none = 0;

    SATURATE(setVal->kp_des, 0.0f, 25.599f);
    SATURATE(setVal->kd_des, 0.0f, 25.599f);
    SATURATE(setVal->tor_des, -127.99f, 127.99f);
    SATURATE(setVal->spd_des, -804.00f, 804.00f);
    SATURATE(setVal->pos_des, -411774.0f, 411774.0f);

    cmdMsg->comd.tor_des = (setVal->tor_des * 256);
    cmdMsg->comd.spd_des = (setVal->spd_des * (128 / PI));
    cmdMsg->comd.pos_des = (setVal->pos_des * REDUCTION_RADIO * (16384 / PI));
    cmdMsg->comd.k_pos = (setVal->kp_des * 1280);
    cmdMsg->comd.k_spd = (setVal->kd_des * 1280);

    cmdMsg->CRC16Data = crc_ccitt(0x0000, (uint8_t *)cmdMsg, 15);

    return 0;
}

int unpackMsg2GetVal(const GO_M8010_6_MotorData *motorMsg, MotorGetValTypeDef *getVal)
{
    if (motorMsg->CRC16Data == crc_ccitt(0x0000, (uint8_t *)motorMsg, 14))
    {
        // getVal->motor_id = motorMsg->mode.id;
        getVal->mode = motorMsg->mode.status >> 1;

        getVal->temp = motorMsg->fbk.temp;
        getVal->merror = motorMsg->fbk.MError;
        getVal->tau = (float)motorMsg->fbk.torque / 256;
        getVal->dq = (float)motorMsg->fbk.speed * PI / 128;
        getVal->q = (float)motorMsg->fbk.pos * PI / 16384 / REDUCTION_RADIO;

        getVal->correct = 1;
        return getVal->correct;
    }
    else
    {
        getVal->correct = 0;
        return getVal->correct;
    }
}
/**
HAL_StatusTypeDef Motor_GO_M8010_Send_Receive(Device_Motor_t *dev_motor_t, Motor_COM_t *motor_com, SemaphoreHandle_t xMotor485IDLESemaphore)
{
    packSetVal2Msg(&motor_com->motor_cmd_msg, &dev_motor_t->motor_cmd);
    HAL_UART_Transmit_DMA(motor_com->motor_huart, (uint8_t *)&motor_com->motor_cmd_msg, sizeof(motor_com->motor_cmd_msg));
    if (xSemaphoreTake(xMotor485IDLESemaphore, pdMS_TO_TICKS(100)) == pdFALSE)
    {
        dev_motor_t->packetLossCount++;
        HAL_UARTEx_ReceiveToIdle_DMA(
            motor_com->motor_huart,
            (uint8_t *)&motor_com->motor_fdk_msg,
            sizeof(motor_com->motor_fdk_msg));
        return HAL_ERROR;
    }
    else
    {
        unpackMsg2GetVal(&motor_com->motor_fdk_msg, &dev_motor_t->motor_state);
        HAL_UARTEx_ReceiveToIdle_DMA(
            motor_com->motor_huart,
            (uint8_t *)&motor_com->motor_fdk_msg,
            sizeof(motor_com->motor_fdk_msg));
        return HAL_OK;
    }
}

void Motor_GO_M8010_Update_State(Device_Motor_t *dev_motor_t, Motor_COM_t *motor_com, SemaphoreHandle_t xMotor485IDLESemaphore)
{
    for (int i = 0; i < 100; i++)
    {
        Motor_GO_M8010_Send_Receive(dev_motor_t, motor_com, xMotor485IDLESemaphore);
    }
}

void Motor_GO_M8010_SetZeroPosition(Device_Motor_t *dev_motor_t, Motor_COM_t *motor_com, SemaphoreHandle_t xMotor485IDLESemaphore)
{
    Trajectory_Init(&dev_motor_t->trajectoryPlanner, dev_motor_t->motor_state.q, 0);

    while (!Trajectory_IsComplete(&dev_motor_t->trajectoryPlanner))
    {
        // 获取插值后的目标位置
        float interpolated_position = Trajectory_GetTargetPosition(&dev_motor_t->trajectoryPlanner);
        dev_motor_t->motor_cmd.pos_des = interpolated_position;
        if (xSemaphoreTake(motor_com->xArmMotorMsgSendSemaphore, portMAX_DELAY) == pdTRUE)
        {
            Motor_GO_M8010_Send_Receive(dev_motor_t, motor_com, xMotor485IDLESemaphore);
        }
        // 更新轨迹
        Trajectory_Update(&dev_motor_t->trajectoryPlanner);
    }
}
**/

/**
 * @brief Send and receive data from a motor device.
 *
 * This function sends a command message to a motor device and starts a DMA receive to get the feedback message.
 * It then checks if the received message is valid and unpacks the data into the motor state.
 *
 * @param dev_motor_t Pointer to the motor device structure.
 * @param motor_com Pointer to the motor communication structure.
 *
 * @return HAL_StatusTypeDef HAL_OK if the operation is successful, HAL_ERROR otherwise.
 */
HAL_StatusTypeDef Motor_GO_M8010_Send_Receive(Device_Motor_t *dev_motor_t, Motor_COM_t *motor_com)
{
    packSetVal2Msg(&motor_com->motor_cmd_msg, &dev_motor_t->motor_cmd); // pack the data into the message

    HAL_UARTEx_ReceiveToIdle_DMA(
        motor_com->motor_huart,
        (uint8_t *)&motor_com->motor_fdk_msg,
        sizeof(motor_com->motor_fdk_msg));            // start the DMA receive
    __HAL_DMA_DISABLE_IT(&hdma_usart2_rx, DMA_IT_HT); // close half transfer interrupt

    HAL_UART_Transmit_DMA(motor_com->motor_huart, (uint8_t *)&motor_com->motor_cmd_msg, sizeof(motor_com->motor_cmd_msg)); // transmit the data

    /*
        motor_com->TimeOutFlag = 0;
        HAL_TIM_Base_Start_IT(&htim7);
        while (1)
        {

            if (motor_com->getFDKFlag == 1 && motor_com->TimeOutFlag == 0)
            {
                HAL_TIM_Base_Stop_IT(&htim7);
                motor_com->getFDKFlag = 0;
                uint8_t *rp = (uint8_t *)&motor_com->motor_fdk_msg;
                if (rp[0] == 0xFD && rp[1] == 0XEE && (motor_com->motor_fdk_msg.mode.id == (uint8_t)dev_motor_t->motor_state.motor_id)) // check the meesage is from the right motor and ensure the message is correct
                {
                    unpackMsg2GetVal(&motor_com->motor_fdk_msg, &dev_motor_t->motor_state); // unpack the data from the message

                    return HAL_OK;
                }
            }
            else if (motor_com->TimeOutFlag == 1)
            {
                HAL_TIM_Base_Stop_IT(&htim7);
                motor_com->TimeOutFlag = 0;
                dev_motor_t->packetLossCount++;
                return HAL_TIMEOUT;
            }
        }
    */

    delay_us(100); // delay for the message to be sent
    if (motor_com->getFDKFlag == 0)
    {
        dev_motor_t->packetLossCount++;
        return HAL_ERROR;
    }
    else if (motor_com->getFDKFlag == 1)
    {
        motor_com->getFDKFlag = 0;
        uint8_t *rp = (uint8_t *)&motor_com->motor_fdk_msg;
        delay_us(1);
        //if (rp[0] == 0xFD && rp[1] == 0XEE && (motor_com->motor_fdk_msg.mode.id == (uint8_t)dev_motor_t->motor_state.motor_id)) // check the meesage is from the right motor and ensure the message is correct
        //{
            unpackMsg2GetVal(&motor_com->motor_fdk_msg, &dev_motor_t->motor_state); // unpack the data from the message
            return HAL_OK;
        //}
        //return HAL_ERROR;
    }

    return HAL_ERROR;
}

void Motor_GO_M8010_Set_Work_KP_KD(Device_Motor_t *dev_motor_t, float kp, float kd)
{
    dev_motor_t->motor_cmd.kp_des = kp;
    dev_motor_t->motor_cmd.kd_des = kd;
}
